/*-----------------------------------------------------------------------------------*/
/* Nuvoton Technology Corporation confidential                                       */
/*                                                                                   */
/* Copyright (c) 2008 by Nuvoton Technology Corporation                              */
/* All rights reserved                                                               */
/*                                                                                   */
/*-----------------------------------------------------------------------------------*/
#ifdef ECOS
#include "string.h"
#include "drv_api.h"
#include "diag.h"
#include "wbtypes.h"
#include "wbio.h"
#else
#include <string.h>
#include "wblib.h"
#endif

#include "nuc900_reg.h"
#include "fmi.h"
#include "nuc900_sic.h"

/* global variable */
char *_ms_gpCopyPage;
__align(4096) char _ms_gBuffer[512];
__align(4096) unsigned short _ms_gLPTable[496];
__align(4096) unsigned short _ms_gFreeTable[512];
int volatile _ms_gSegCount, _ms_gBootBlockIndex;
int volatile _ms_gSegIndex, _ms_gMSProWriteRegSize;

FMI_MSPRO_DEVICE_INFO_ENTRY	_ms_gMSProDeviceEntry;
FMI_MSPRO_SYS_INFO _ms_gMSProSystemInfo;

struct msSegment msPhySeg[] = 
{
	{    0,  494 },	// segment 0
	{  494,  990 },	// segment 1
	{  990, 1486 },	// segment 2
	{ 1486, 1982 },	// segment 3
	{ 1982, 2478 },	// segment 4
	{ 2478, 2974 },	// segment 5
	{ 2974, 3470 },	// segment 6
	{ 3470, 3966 },	// segment 7
	{ 3966, 4462 },	// segment 8
	{ 4462, 4958 },	// segment 9
	{ 4958, 5454 },	// segment 10
	{ 5454, 5950 },	// segment 11
	{ 5950, 6446 },	// segment 12
	{ 6446, 6942 },	// segment 13
	{ 6942, 7438 },	// segment 14
	{ 7438, 7934 }	// segment 15
};


/* functions */
/* count : for short data
			0x00 => 32 bytes
			0x01 => 64 bytes
			0x02 => 128 bytes
			0x03 => 256 bytes */
int msSendTPC(FMI_MS_INFO_T *pMS, INT tpc_cmd, INT count, char *buf)
{
	int volatile i;
	char *pReg;

	// disable INT timeout interrupt
	outpw(REG_MSIER, inpw(REG_MSIER) & 0xfffffffb);

	_fmi_bIsMSDataReady = FALSE;

	switch (tpc_cmd)
	{
		case MS_READ_LONG_DATA:		// READ_PAGE_DATA
		case MS_WRITE_LONG_DATA:	// WRITE_PAGE_DATA
			// enable INT timeout interrupt
			_fmi_bIsMSTimeOut = FALSE;
			outpw(REG_MSIER, inpw(REG_MSIER) | 0x04);

			// enable DMAC
			outpw(REG_DMACCSR, 0x01);

			// set DMA transfer starting address
			outpw(REG_DMACSAR2, (INT)buf);
			break;

		case MS_READ_SHORT_DATA:
		case MS_WRITE_SHORT_DATA:
			// enable DMAC
			outpw(REG_DMACCSR, 0x01);

			// set DMA transfer starting address
			outpw(REG_DMACSAR2, (INT)buf);

			// set data size
			outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xffe7ffff)|((count & 0x3)<<19));
			break;

		case MS_WRITE_REG:
		case MS_SET_RW_REG_ADRS:
			// set data count
			outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfff8ffff)|((count & 0x7)<<16));
			if (count == 0)
				break;
			pReg = (char *)(REG_MSBUF2+3);
			for (i=0; i<count; i++)
			{
				*pReg = *buf;
				pReg--;
				buf++;
			}
			break;

		case MS_READ_REG:
		case MS_GET_INT:
		default:
			// set data count
			outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfff8ffff)|((count & 0x7)<<16));
	}
	// write tpc command
	outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfffff0ff)|(tpc_cmd << 8));
	outpw(REG_MSCSR, inpw(REG_MSCSR)|0x02);

	if ((tpc_cmd == MS_READ_REG) || (tpc_cmd == MS_GET_INT))
	{
		while(!_fmi_bIsMSDataReady)
		{
			if (pMS->bIsCardInsert == FALSE)
				return FMI_NO_MS_CARD;
		}
		pReg = (char *)(REG_MSBUF2+3);
		for (i=0; i<count; i++)
		{
			*buf = *pReg;
			pReg--;
			buf++;
		}
	}

	return Successful;
}

int msSetCMD(INT cmdCode)
{
	_fmi_bIsMSDataReady = FALSE;

	if (cmdCode != MS_RESET)
	{
		// enable INT timeout interrupt
		_fmi_bIsMSTimeOut = FALSE;
		outpw(REG_MSIER, inpw(REG_MSIER) | 0x04);
	}

	// set command code
	outpw(REG_MSBUF2, cmdCode << 24);

	// set data count = 1
	outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfff8ffff)|(1<<16));

	// write tpc command
	outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfffff0ff)|(MS_SET_CMD << 8));
	outpw(REG_MSCSR, inpw(REG_MSCSR)|0x02);

	return Successful;
}

int msExSetCMD(INT cmdCode)
{
	_fmi_bIsMSDataReady = FALSE;

	// enable INT timeout interrupt
	_fmi_bIsMSTimeOut = FALSE;
	outpw(REG_MSIER, inpw(REG_MSIER) | 0x04);

	// set command code
	outpw(REG_MSBUF2, (inpw(REG_MSBUF2)&0x00ffffff) | (cmdCode << 24));

	// set data count = 7
	outpw(REG_MSCSR, inpw(REG_MSCSR)|0x70000);

	// write tpc command
	outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfffff0ff)|(MS_EX_SET_CMD << 8));
	outpw(REG_MSCSR, inpw(REG_MSCSR)|0x02);

	return Successful;
}

int msWaitINT(FMI_MS_INFO_T *pMS)
{
	while(!(inpw(REG_MSISR) & 0x02))
	{
		if(_fmi_bIsMSTimeOut)
		{
			outpw(REG_MSISR, 0x06);
			return FMI_MS_INT_TIMEOUT;	// check INT response timeout				
		}

		if (pMS->bIsCardInsert == FALSE)
			return FMI_NO_MS_CARD;
	}
	outpw(REG_MSISR, 0x06);
	return Successful;	
}

int msWaitPacketTransfer(FMI_MS_INFO_T *pMS)
{
	/* wait for packet transferred */
	while(!_fmi_bIsMSDataReady)
	{
		if (pMS->bIsCardInsert == FALSE)
			return FMI_NO_MS_CARD;
	}

	/* check RDY signal */
	if(inpw(REG_MSISR) & 0x08)
	{
		outpw(REG_MSISR, 0x08);
		return FMI_MS_BUSY_TIMEOUT;	// timeout
	}

	/* check CRC error */
	if(inpw(REG_MSISR) & 0x10)
	{
		outpw(REG_MSISR, 0x10);
		return FMI_MS_CRC_ERROR;
	}
	return Successful;	
}

//===============================================
//	input:
//		1. BlockAddr (physical address)
//		2. PageAddr (page No.)
//		3. pUploadBuf (upload pointer)
//		4. pExtraBuf (Extra Data Area pointer)
//===============================================
int msReadOnePage(FMI_MS_INFO_T *pMS, int BlockAddr, int PageAddr, char *pUploadBuf, char *pExtraBuf)
{
	char baBuf[8];
	int volatile reg, status;

	/* read INT reg, status reg0, status reg1 */
	baBuf[0] = 0x01;	// read address
    baBuf[1] = 0x03;	// read size
	/* write block address reg, CMD reg, and page address reg */
    baBuf[2] = 0x11;	// write address
    baBuf[3] = 0x05;	// write size

    /* send SET_R/W_REG_ADRS Command */
    msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	baBuf[0] = (BlockAddr & 0x00FF0000) >>16;	// block address 2 (MSB)
    baBuf[1] = (BlockAddr & 0x0000FF00) >> 8;	// block address 1
    baBuf[2] = (BlockAddr & 0x000000FF);		// block address 0 (LSB)
    baBuf[3] = 0x20;		// command parameter; Single Page Access Mode
    baBuf[4] = PageAddr;	// page address

    /* send WRITE_REG Command */
    msSendTPC(pMS, MS_WRITE_REG, 0x05, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send BLOCK READ command */		                                             
	msSetCMD(MS_BLOCK_READ);

	/* wait INT ready */		                                                     
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful) return status;

	/* check INT register */
	msSendTPC(pMS, MS_READ_REG, 0x03, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;			// INT register

	if(reg & 0x01) return FMI_MS_INT_CMDNK;	// Command error
	if(reg & 0x40) return FMI_MS_INT_ERR;	// Read error
	if(!(reg & 0x20)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?

	reg = *(baBuf+2);	// SATUS1 register

	if(!(reg & 0x15))	// no error in data???
	{
		/* read overwrite flag, management flag, logical address */
		baBuf[0] = 0x16;	// read address
		baBuf[1] = 0x04;	// read size
		/* write block address reg, CMD reg, and page address reg */
		baBuf[2] = 0x11;	// write address
		baBuf[3] = 0x05;	// write size

		/* send SET_R/W_REG_ADRS Command */
		msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

		/* check RDY signal */		              
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	    /* check OverWrite Flag register */
		msSendTPC(pMS, MS_READ_REG, 0x04, pExtraBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		// read page data
		msSendTPC(pMS, MS_READ_PAGE_DATA, 0x00, pUploadBuf);	// receive data from data buffer

		/* check RDY signal */		              
		while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
		{
			if (pMS->bIsCardInsert == FALSE)
			{
				outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
				return FMI_NO_MS_CARD;
			}
		}

		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
		// clear timeout flag
		outpw(REG_MSISR, inpw(REG_MSISR) | 0x04);

		reg = *pExtraBuf;	// get OverWrite register

		/* check Block status and Page status */
		if((reg & 0x80) && (reg & 0x60))
			return Successful;
	}
	return FMI_MS_READ_PAGE_ERROR;	
}

//========================================
//	input:
//		1. BlockAddr (physical address)
//		2. PageAddr (page No.)
//		3. pUploadBuf (upload pointer)
//========================================
int msReadOnePageExtraData(FMI_MS_INFO_T *pMS, int BlockAddr, int PageAddr, char *pUploadBuf)
{
	char baBuf[8];
	int volatile reg, status;

	baBuf[0] = 0x01;	// read address
	baBuf[1] = 0x03;	// read size
	baBuf[2] = 0x11;	// write address
	baBuf[3] = 0x05;	// write size

	/* send SET_R/W_REG_ADRS Command */
	msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	baBuf[0] = (BlockAddr & 0x00FF0000) >>16;	// block address 2 (MSB)                             
	baBuf[1] = (BlockAddr & 0x0000FF00) >> 8;	// block address 1
	baBuf[2] = (BlockAddr & 0x000000FF);		// block address 0 (LSB)                             
	baBuf[3] = 0x40;		// command parameter; Extra Data Access Mode        
	baBuf[4] = PageAddr;		// page address                                      

	/* send WRITE_REG Command */		              	                             
	msSendTPC(pMS, MS_WRITE_REG, 0x05, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
                                                                                     
	/* send BLOCK READ command */
	msSetCMD(MS_BLOCK_READ);

	/* wait INT ready */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful)	return status;

	/* check INT register */
	msSendTPC(pMS, MS_READ_REG, 0x03, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;	// INT register

	if(reg & 0x01) return FMI_MS_INT_CMDNK;	// Command error
	if(reg & 0x04) return FMI_MS_INT_ERR;	// Read error

	reg = *(baBuf+2);		// SATUS1 register

	if(!(reg & 0x05))		// no error in registe???
	{
		baBuf[0] = 0x16;	// read address
		baBuf[1] = 0x04;	// read size
		baBuf[2] = 0x11;	// write address
		baBuf[3] = 0x05;	// write size

		/* send SET_R/W_REG_ADRS Command */
		msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		/* read Extra Data Area */
		msSendTPC(pMS, MS_READ_REG, 0x04, pUploadBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	}
	return Successful;
}

//===============================================
//	input:
//		1. BlockAddr (physical address)
//		2. PageAddr (page No.)
//		3. pLoadBuf (load pointer)
//		4. pExtraBuf (Extra Data Area pointer)
//===============================================
int msWriteOnePage(FMI_MS_INFO_T *pMS, int BlockAddr, int PageAddr, char *pLoadBuf, char *pExtraBuf)
{
	char baBuf[8];
	int volatile reg, status;

	/* read INT reg */
	baBuf[0] = 0x01;	// read address
    baBuf[1] = 0x01;	// read size
	/* write block address reg, CMD reg, and page address reg */
    baBuf[2] = 0x11;	// write address
    baBuf[3] = 0x05;	// write size

    /* send SET_R/W_REG_ADRS Command */
    msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	baBuf[0] = (BlockAddr & 0x00FF0000) >>16;	// block address 2 (MSB)
    baBuf[1] = (BlockAddr & 0x0000FF00) >> 8;	// block address 1
    baBuf[2] = (BlockAddr & 0x000000FF);		// block address 0 (LSB)
    baBuf[3] = 0x20;		// command parameter; Single Page Access Mode
    baBuf[4] = PageAddr;	// page address

    /* send WRITE_REG Command */
    msSendTPC(pMS, MS_WRITE_REG, 0x05, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* write Logical Address */
	if (!PageAddr)
	{
		baBuf[0] = 0x01;	// read address
		baBuf[1] = 0x01;	// read size
		baBuf[2] = 0x16;	// write address
		baBuf[3] = 0x04;	// write size

		/* send SET_R/W_REG_ADRS Command */
		msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		/* send WRITE_REG Command */
		msSendTPC(pMS, MS_WRITE_REG, 0x04, pExtraBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	}

	/* send BLOCK WRITE command */
	msSetCMD(MS_BLOCK_WRITE);

	/* wait INT ready */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful)	return status;

	/* check INT register */
	msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;	// INT register
	if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
	if(!(reg & 0x20)) return FMI_MS_INT_BREQ;		// Data is ready in Data Buffer?

	msSendTPC(pMS, MS_WRITE_PAGE_DATA, 0x00, pLoadBuf);	// send data from data buffer

	/* wait DMAC ready */
	while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
	{
		if (pMS->bIsCardInsert == FALSE)
		{
			outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
			return FMI_NO_MS_CARD;
		}
	}

	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful)	return status;

	/* check INT register */
	msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;			// INT register
	if(!(reg & 0x80)) return FMI_MS_INT_CED_ERR;	// Commad finished?
	if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
	if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error

	return Successful;
}

//===============================================
//	input:
//		1. BlockAddr (physical address)
//		2. PageAddr (page No.)
//		3. pLoadBuf (load pointer)
//===============================================
int msWriteOnePageExtraData(FMI_MS_INFO_T *pMS, int BlockAddr, int PageAddr, char *pLoadBuf)
{
	char baBuf[8];
	int volatile reg, status;

	/* read INT reg */
	baBuf[0] = 0x01;	// read address
    baBuf[1] = 0x01;	// read size
	/* write block address reg, CMD reg, and page address reg */
    baBuf[2] = 0x11;	// write address
    baBuf[3] = 0x05;	// write size

    /* send SET_R/W_REG_ADRS Command */
    msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	baBuf[0] = (BlockAddr & 0x00FF0000) >>16;	// block address 2 (MSB)
    baBuf[1] = (BlockAddr & 0x0000FF00) >> 8;	// block address 1
    baBuf[2] = (BlockAddr & 0x000000FF);		// block address 0 (LSB)
	baBuf[3] = 0x40;		// command parameter; Extra Data Access Mode        
    baBuf[4] = PageAddr;	// page address

    /* send WRITE_REG Command */
    msSendTPC(pMS, MS_WRITE_REG, 0x05, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	baBuf[0] = 0x01;	// read address
	baBuf[1] = 0x01;	// read size
	baBuf[2] = 0x16;	// write address
	baBuf[3] = 0x04;	// write size

	/* send SET_R/W_REG_ADRS Command */
	msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send WRITE_REG Command */
	msSendTPC(pMS, MS_WRITE_REG, 0x04, pLoadBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send BLOCK WRITE command */
	msSetCMD(MS_BLOCK_WRITE);

	/* wait INT ready */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful)	return status;

	/* check INT register */
	msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;			// INT register
	if(!(reg & 0x80)) return FMI_MS_INT_CED_ERR;		// Commad finished?
	if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
	if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error

	return Successful;
}

//=======================================
//	input:
//		1. wBlockAddr (physical address)
//=======================================
int msEraseBlock(FMI_MS_INFO_T *pMS, int BlockAddr)
{
	char baBuf[8];
	int volatile reg, status;

	/* read INT reg */
	baBuf[0] = 0x01;	// read address
    baBuf[1] = 0x08;	// read size
	/* write block address reg, CMD reg, and page address reg */
    baBuf[2] = 0x11;	// write address
    baBuf[3] = 0x03;	// write size

    /* send SET_R/W_REG_ADRS Command */
    msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	baBuf[0] = (BlockAddr & 0x00FF0000) >>16;	// block address 2 (MSB)
    baBuf[1] = (BlockAddr & 0x0000FF00) >> 8;	// block address 1
    baBuf[2] = (BlockAddr & 0x000000FF);		// block address 0 (LSB)

	/* send WRITE_REG Command */
	msSendTPC(pMS, MS_WRITE_REG, 0x03, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send BLOCK ERASE command */		                                             
	msSetCMD(MS_BLOCK_ERASE);

	/* wait INT ready */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful)	return status;

	/* check INT register */
	msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;			// INT register
	if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error
	if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
	if(!(reg & 0x80)) return FMI_MS_INT_CED_ERR;	// Commad finished?

	return Successful;
}

int msCopyOnePage(FMI_MS_INFO_T *pMS, int SrcBlockAddr, int SrcPageAddr, int DestBlockAddr, int DestPageAddr)
{
	char baExtraData[4];
	char baBuf[8];      		
	int reg, status;

	baBuf[0] = 0x01;	// read address
	baBuf[1] = 0x03;	// read size
	baBuf[2] = 0x11;	// write address
	baBuf[3] = 0x05;	// write size
                                                   
	/* send SET_R/W_REG_ADRS Command */
	msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	baBuf[0] = 0x00;				// block address 2 (MSB)
	baBuf[1] = (SrcBlockAddr & 0x0000FF00) >> 8;	// block address 1
	baBuf[2] = (SrcBlockAddr & 0x000000FF);		// block address 0 (LSB)
	baBuf[3] = 0x20;			// command parameter; Single Page Access Mode
	baBuf[4] = SrcPageAddr;	// page address                                      

	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send WRITE_REG Command */
	msSendTPC(pMS, MS_WRITE_REG, 0x05, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send BLOCK READ command */
	msSetCMD(MS_BLOCK_READ);

	/* wait INT ready */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful)	return status;

	/* check INT register */
	msSendTPC(pMS, MS_READ_REG, 0x03, baBuf);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	reg = *baBuf;			// INT register

	if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
	if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error
	if(!(reg & 0x20)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer

	reg = *(baBuf+2);				// SATUS1 register

	if(!(reg & 0x15))		// no error in data???
	{
		baBuf[0] = 0x16;	// read address
		baBuf[1] = 0x04;	// read size
		baBuf[2] = 0x16;	// write address
		baBuf[3] = 0x04;	// write size

		/* send SET_R/W_REG_ADRS Command */
		msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		/* read Extra Data Area */
		msSendTPC(pMS, MS_READ_REG, 0x04, baExtraData);
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		/* read Page Data from MS */
		msSendTPC(pMS, MS_READ_PAGE_DATA, 0x00, _ms_gpCopyPage);	// receive data from data buffer
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		// clear timeout flag
		outpw(REG_MSISR, inpw(REG_MSISR) | 0x04);

		/* check RDY signal */
		while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
		{
			if (pMS->bIsCardInsert == FALSE)
			{
				outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
				return FMI_NO_MS_CARD;
			}
		}

		/* write Page Data to MS */
		msSendTPC(pMS, MS_WRITE_PAGE_DATA, 0x00, _ms_gpCopyPage);	// send data from data buffer
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		/* wait INT ready */
		while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
		{
			if (pMS->bIsCardInsert == FALSE)
			{
				outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
				return FMI_NO_MS_CARD;
			}
		}

		/* write Extra Data Area */
		msSendTPC(pMS, MS_WRITE_REG, 0x04, baExtraData);
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		/* send BLOCK WRITE command */
		msSetCMD(MS_BLOCK_WRITE);
	                                                                                     
		/* wait INT ready */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
		if((status = msWaitINT(pMS)) != Successful)	return status;

		/* check INT register */
		msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		reg = *baBuf;				// INT register
		if(!(reg & 0x80)) return FMI_MS_INT_CED_ERR;	// Commad finished?
		if(reg & 0x01) return FMI_MS_INT_CMDNK;	// Command error
		if(reg & 0x40) return FMI_MS_INT_ERR;	// Read error

		return Successful;
	}
	else
	{
		// data error
		/* read Page Data from MS */
		msSendTPC(pMS, MS_READ_PAGE_DATA, 0x00, _ms_gpCopyPage);	// receive data from data buffer

		/* check RDY signal */		              
		while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
		{
			if (pMS->bIsCardInsert == FALSE)
			{
				outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
				return FMI_NO_MS_CARD;
			}
		}

		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		return FMI_MS_COPY_PAGE_ERR;
	}
}

//=============================================
//	add the new erased block to Free_Table[]
//=============================================
int msAllocateFreeTable(int NewErasedBlockAddr)
{
	int volatile i;

	for (i=0; i<512; i++)
	{
		if (_ms_gFreeTable[i] == 0xFFFF)
		{
			_ms_gFreeTable[i] = NewErasedBlockAddr;
			return Successful;
		}
	}
	return FMI_MS_ALLOC_ERR;
}

//==========================================
//	search new unused block for data copy
//==========================================
int msAllocateUnusedBlock(void)
{
	int UnusedPhyBlockAddr;
	int volatile i;

	UnusedPhyBlockAddr = 0xFFFF;
	for (i=0; i<512; i++)
	{
		if (_ms_gFreeTable[i] != 0xFFFF)
		{
			UnusedPhyBlockAddr = _ms_gFreeTable[i];
			_ms_gFreeTable[i] = 0xFFFF;
			break;
		}
	}

	return UnusedPhyBlockAddr;
}

int msMakeTable(FMI_MS_INFO_T *pMS, int SegIndex)
{
	char pBuf[4], pBuf2[4];
	int volatile Idx, status, logBlock;
	int StartPhyBlock, EndPhyBlock, PhyBlock_P;

	/* verify input segment index to be OK */
	if(SegIndex > _ms_gSegCount) return FMI_MS_WRONG_SEGMENT;	// wrong bSegIndex

	/* confirm the start/end physical block in current segment */
	if (SegIndex == 0)	
	{
		StartPhyBlock = _ms_gBootBlockIndex + 1;	// start PhyBlock after two boot block
		EndPhyBlock = 511;
	}
	else
	{
		StartPhyBlock = SegIndex * 0x200;
		EndPhyBlock = StartPhyBlock + 0x200 - 1;
	}

	/* initialize LP Table and Free Table to 0xFFFF */
    memset(_ms_gLPTable, 0xFFFF, 496*2);
    memset(_ms_gFreeTable, 0xFFFF, 512*2);

	/* create LP Table and Free Table for current segment */
	Idx = 0;

	PhyBlock_P = StartPhyBlock;
	while(1)
	{
		if (pMS->bIsCardInsert == FALSE)
			return FMI_NO_MS_CARD;

		if (PhyBlock_P > EndPhyBlock)
			break;	
		
//		if (PhyBlock_P == 10)
//		{
//			status = 0;
//		}
		status = msReadOnePageExtraData(pMS, PhyBlock_P, 0x00, pBuf);

		if (status) goto nextBlock;
		if(!(pBuf[0] & 0x80))	goto nextBlock;		// check Block Status
		if((pBuf[0] & 0x60)==0x00)	goto nextBlock;	// Data Error
		if((pBuf[0] & 0x60)==0x20)	goto nextBlock;	// Access Protected
		if((pBuf[0] & 0x60)==0x40)	goto nextBlock;	// Undefined
		
		// current block_P is OK, read Logical Address
		logBlock = (pBuf[2] << 8) | pBuf[3];
		if (logBlock != 0xFFFF)
			logBlock = logBlock - (SegIndex*496);	// offset value for LP_Table[]

		// check blockIndex = 0xFFFF or in pre-defined range
		if ((logBlock == 0xFFFF) ||
		   ((logBlock < msPhySeg[SegIndex].msStartLogicalBlock) ||
		    (logBlock > msPhySeg[SegIndex].msEndLogicalBlock)) )
		{
			msEraseBlock(pMS, PhyBlock_P);
			_ms_gFreeTable[Idx] = PhyBlock_P;	// register current block on Free Table[n]
			Idx++;
			goto nextBlock;
		}

		// check if one Logical Address mapped to two different Physical Address */
		if (_ms_gLPTable[logBlock] == 0xFFFF)
		{
			_ms_gLPTable[logBlock] = PhyBlock_P;
			goto nextBlock;
		}

		// one Logical Address mapped to two different Physical Address */
		status = msReadOnePageExtraData(pMS, _ms_gLPTable[logBlock], 0x00, pBuf2);
		if (status) goto nextBlock;

		// comapare Update Status (US) of LP Table[L] with US of block_P		
		if ((pBuf2[0]&0x10) == (pBuf[0]&0x10))
		{
			if (PhyBlock_P < _ms_gLPTable[logBlock])
			{
				msEraseBlock(pMS, PhyBlock_P);				
				_ms_gFreeTable[Idx] = PhyBlock_P;			// register wPhyBlock_P on Free Table[n]
				Idx++;
				goto nextBlock;
			}
			else
			{
				msEraseBlock(pMS, _ms_gLPTable[logBlock]);
				_ms_gFreeTable[Idx] = _ms_gLPTable[logBlock];	// register LP Table[L] on Free Table[n]
				_ms_gLPTable[logBlock] = PhyBlock_P;
				Idx++;				
				goto nextBlock;			 
			}
		}
		// US of LP Table[L] is different from US of block_P		
		else
		{
			// check US of block_P = 0
			if ((pBuf[0]&0x10) != 0x00)			
			{
				msEraseBlock(pMS, PhyBlock_P);				
				_ms_gFreeTable[Idx] = PhyBlock_P;	// register wPhyBlock_P on Free Table[n]
				Idx++;
				goto nextBlock;
			}
			else
			{
				msEraseBlock(pMS, _ms_gLPTable[logBlock]);
				_ms_gFreeTable[Idx] = _ms_gLPTable[logBlock];	// register LP Table[L] on Free Table[n]
				_ms_gLPTable[logBlock] = PhyBlock_P;
				Idx++;
				goto nextBlock;			 
			}
		}
nextBlock:
		PhyBlock_P++;
	}			
	return Successful;
}

//=========================
//	input:
//		1. pUploadBuf
//		2. SectorNo
//		3. SectorCount
//=========================
int msReadLogicalPages(FMI_MS_INFO_T *pMS, int SectorNo, int SectorCount, char *pUploadBuf)
{
	int LogBlockAddr, PhyBlockAddr, idx, PageAddr, SegIndex;
	char baExtra[4];
	int volatile status=0;

	//printf("read: sectorNo [%x], count[%x]\n", SectorNo, SectorCount);

	PageAddr = 0;
	while (SectorCount)
	{
		/* translate Logical Sector to Physical Block/Page Address */
		if (PageAddr == 0)						// 1st sector to be read
		{
			LogBlockAddr = SectorNo / pMS->uSectorPerBlock;
			PageAddr  = SectorNo % pMS->uSectorPerBlock;
			SegIndex=0;
			
			if(LogBlockAddr >= 494)
			{
				LogBlockAddr -= 494;	// logical sector count of 1st segment 

				SegIndex = LogBlockAddr / 496;
				LogBlockAddr  = LogBlockAddr % 496;
	
				SegIndex += 1;					// add first segment (segment-0) 
				if (SegIndex > 15)	return FMI_MS_WRONG_SEGMENT;	// more than maximum sector size to be read
			}
		}
		else if (PageAddr == pMS->uSectorPerBlock)	// not 1st sector to be read
		{
			PageAddr = 0;
			LogBlockAddr += 1;

			idx = msPhySeg[SegIndex].msEndLogicalBlock - msPhySeg[SegIndex].msStartLogicalBlock;
			if (LogBlockAddr == idx)			// beyond the block boundary of currnt SegIndex
			{
				LogBlockAddr = 0;
				SegIndex += 1;
				if (SegIndex > 15)	return FMI_MS_WRONG_SEGMENT;	// more than maximum sector size to be read
			}
		}

		/* create new mapiing table if necessary */
		if(SegIndex != _ms_gSegIndex)	
		{
			msMakeTable(pMS, SegIndex);	// create new Logical/Physical mapping table
			_ms_gSegIndex = SegIndex;
		}
	
		/* read sector data from Physical Block and Page Address */
		PhyBlockAddr = _ms_gLPTable[LogBlockAddr];
		if (PhyBlockAddr == 0xFFFF)	return FMI_MS_WRONG_PHYBLOCK;

		status = msReadOnePage(pMS, PhyBlockAddr, PageAddr, pUploadBuf, baExtra);
		if (status != Successful)
			return status;
		status = msReadOnePageExtraData(pMS, PhyBlockAddr, 0x00, baExtra);			
		if (status != Successful)
			return status;

		SectorNo +=1;
		pUploadBuf += 0x200;
		SectorCount -= 1;
		PageAddr +=1; 
	}
	return Successful;
}

//=========================
//	input:
//		1. pLoadBuf
//		2. SectorNo
//		3. SectorCount
//=========================
int msWriteLogicalPages(FMI_MS_INFO_T *pMS, int SectorNo, int SectorCount, char *pLoadBuf)
{
	unsigned int LogBlockAddr, PhyBlockAddr, UnusedPhyBlockAddr, Lblock;
	unsigned int LogBlockAddr_backup;
	int volatile PageAddr, SegIndex, idx, status=0;
	char baExtra[4];

	//printf("write: sectorNo [%x], count[%x]\n", SectorNo, SectorCount);

	PageAddr = 0;

	/* 1. translate Logical Sector to Physical Block/Page Address */ 
	LogBlockAddr = SectorNo / pMS->uSectorPerBlock;	// 1st sector to be written 
	LogBlockAddr_backup = LogBlockAddr;			// for update Logical Address of block to be updated
	PageAddr  = SectorNo % pMS->uSectorPerBlock;
	SegIndex=0;
	
	if(LogBlockAddr >= 494)
	{
		LogBlockAddr -= 494;			// logical sector count of 1st segment 

		SegIndex = LogBlockAddr / 496;
		LogBlockAddr  = LogBlockAddr % 496;

		SegIndex += 1;					// add first segment (segment-0) 
		if (SegIndex > 15)	return FMI_MS_WRONG_SEGMENT;	// more than maximum sector size to be accessed
	}

	if(SegIndex != _ms_gSegIndex)		// create new mapiing table if necessary 
	{
		msMakeTable(pMS, SegIndex);	// create new Logical/Physical mapping table
		_ms_gSegIndex = SegIndex;
	}

#if 0
	/* 2. change update status of block before updated to 0 (:under updating)*/
	msReadOnePageExtraData(PhyBlockAddr, 0x00, baBuf);	// update status on page-0 Extra Data Area
	baBuf &= 0xEF;
	msWriteOnePageExtraData(PhyBlockAddr, 0x00, baBuf);
#endif

	/* 3. search for an unused block for updating */
	UnusedPhyBlockAddr = msAllocateUnusedBlock();
	if (UnusedPhyBlockAddr == 0xFFFF)	return FMI_MS_ALLOC_ERR;	// write error (no unused block can be found)

	/* 4. copy page process is implemented in page No. order from page 0 to previous page before updated page */
	PhyBlockAddr = _ms_gLPTable[LogBlockAddr];		// get corresponding physical block address
	_ms_gLPTable[LogBlockAddr] = UnusedPhyBlockAddr;

	if (PhyBlockAddr == 0xFFFF)					// no available Physical Block to be found
	{
		baExtra[0] = 0xFF;			// Overwrite Flag register 
		baExtra[1] = 0xFF;			// Management Flag register
		baExtra[2] = (char)(LogBlockAddr_backup >> 8);		// Logical Address 1 (MSB)
		baExtra[3] = (char)(LogBlockAddr_backup & 0x00FF);	// Logical Address 0 (LSB)
		status = msWriteOnePageExtraData(pMS, UnusedPhyBlockAddr, 0x00, baExtra);
		if (status != Successful)
			return status;

		// for debug
//		status = msReadOnePageExtraData(pMS, UnusedPhyBlockAddr, 0x00, (char *)0x80300000);
//		if (status != Successful)
//			return status;

		//UnusedPhyBlockAddr = msAllocateUnusedBlock();
		//if (UnusedPhyBlockAddr == 0xFFFF)	return FMI_MS_ALLOC_ERR;	// write error (no unused block can be found)
	}
	else
	{
		// for debug
		for (idx=0; idx<PageAddr; idx++)
		{
			// read back
			status = msReadOnePage(pMS, PhyBlockAddr, idx, _ms_gpCopyPage, baExtra);
			if (status != Successful)
				return status;

			// write into
			baExtra[0] = 0xFF;			// Overwrite Flag register 
			baExtra[1] = 0xFF;			// Management Flag register
			baExtra[2] = (char) (LogBlockAddr_backup >> 8);
			baExtra[3] = (char) (LogBlockAddr_backup & 0x00FF);
			status = msWriteOnePage(pMS, UnusedPhyBlockAddr, idx, _ms_gpCopyPage, baExtra);	// write updated data
			if (status != Successful)
				return status;

//			status = msReadOnePage(pMS, UnusedPhyBlockAddr, idx, (char *)0x80300000, baExtra);
//			if (status != Successful)
//				return status;
		}

#if 0
		for (idx=0; idx<PageAddr; idx++)
		{
			status = msCopyOnePage(pMS, PhyBlockAddr, idx, UnusedPhyBlockAddr, idx);
			if (status != Successful)
				return status;

			// for debug
			status = msReadOnePage(pMS, UnusedPhyBlockAddr, idx, (char *)0x80300000, baExtra);
			if (status != Successful)
				return status;
		}
#endif
	}

	/* 5. write updated data to updated pages */
	baExtra[0] = 0xFF;			// Overwrite Flag register 
	baExtra[1] = 0xFF;			// Management Flag register

	while(SectorCount)
	{
		if (PageAddr < pMS->uSectorPerBlock)	
		{
			if (PageAddr == 0)
			{
				baExtra[2] = (char) (LogBlockAddr_backup >> 8);
				baExtra[3] = (char) (LogBlockAddr_backup & 0x00FF);
				status = msWriteOnePage(pMS, UnusedPhyBlockAddr, PageAddr, pLoadBuf, baExtra);	// write updated data
				if (status != Successful)
					return status;
			}
			else
			{
				status = msWriteOnePage(pMS, UnusedPhyBlockAddr, PageAddr, pLoadBuf, baExtra);	// write updated data
				if (status != Successful)
					return status;
			}
			// for debug
//			status = msReadOnePage(pMS, UnusedPhyBlockAddr, PageAddr, (char *)0x80300000, baExtra);
//			if (status != Successful)
//				return status;
		}
		else
		{
			// update Logical/Physical corresponding information on host product */
			//_ms_gLPTable[LogBlockAddr] = UnusedPhyBlockAddr;

			// erase block before updated */
			if (PhyBlockAddr != 0xFFFF)
			{
				status = msEraseBlock(pMS, PhyBlockAddr);
				if (status != Successful)
					return status;
				status = msAllocateFreeTable(PhyBlockAddr);		// add erased block to Free_Table[]
				if (status != Successful)
					return status;
			}

			// search following logical block address
			LogBlockAddr_backup += 1;					// for update Logical Address of block to be updated
			PageAddr = 0;
			LogBlockAddr += 1;

			Lblock = msPhySeg[SegIndex].msEndLogicalBlock - msPhySeg[SegIndex].msStartLogicalBlock;
			if (LogBlockAddr >= Lblock)			// beyond the block boundary of currnt SegIndex
			{
				LogBlockAddr = 0;
				SegIndex += 1;
				if (SegIndex > 15)	return FMI_MS_WRONG_SEGMENT;	// more than maximum sector size to be accessed
			}

			if(SegIndex != _ms_gSegIndex)		// create new mapiing table if necessary 
			{
				msMakeTable(pMS, SegIndex);	// create new Logical/Physical mapping table
				_ms_gSegIndex = SegIndex;
			}

			// search for an unused block for updating */
			UnusedPhyBlockAddr = msAllocateUnusedBlock(); 
			if (UnusedPhyBlockAddr == 0xFFFF)	return FMI_MS_ALLOC_ERR;	// write error (no unused block can be found)

			PhyBlockAddr = _ms_gLPTable[LogBlockAddr];		// get corresponding physical block address
			_ms_gLPTable[LogBlockAddr] = UnusedPhyBlockAddr;
#if 0
			if (PhyBlockAddr == 0xFFFF)					// no available Physical Block to be found
			{
				PhyBlockAddr = UnusedPhyBlockAddr;
				_ms_gLPTable[LogBlockAddr] = PhyBlockAddr;
				UnusedPhyBlockAddr = msAllocateUnusedBlock();
				if (UnusedPhyBlockAddr == 0xFFFF)	return FMI_MS_ALLOC_ERR;	// write error (no unused block can be found)
			}		
#endif
			// next block page 0 // hpchen
			// update Logical Address in new copied block
			baExtra[2] = (char)(LogBlockAddr_backup >> 8);
			baExtra[3] = (char)(LogBlockAddr_backup & 0x00FF);
			status = msWriteOnePage(pMS, UnusedPhyBlockAddr, PageAddr, pLoadBuf, baExtra);	// write updated data
			if (status != Successful)
				return status;

			// for debug
//			status = msReadOnePage(pMS, UnusedPhyBlockAddr, PageAddr, (char *)0x80300000, baExtra);
//			if (status != Successful)
//				return status;
		}
		pLoadBuf += 0x200;
		SectorCount -= 1;
		PageAddr += 1;
	}

	/* 6. copy page process is implemented in page No. order from next page to last page */
	if (PageAddr < pMS->uSectorPerBlock)		// check if is bPageAddr not equal to g_bSectorPerBlock?
	{
		// for debug
		for (idx=PageAddr; idx<pMS->uSectorPerBlock; idx++)
		{
			// read back
			status = msReadOnePage(pMS, PhyBlockAddr, idx, _ms_gpCopyPage, baExtra);
			if (status != Successful)
				return status;

			// write into
			baExtra[0] = 0xFF;			// Overwrite Flag register 
			baExtra[1] = 0xFF;			// Management Flag register
			baExtra[2] = (char) (LogBlockAddr_backup >> 8);
			baExtra[3] = (char) (LogBlockAddr_backup & 0x00FF);
			status = msWriteOnePage(pMS, UnusedPhyBlockAddr, idx, _ms_gpCopyPage, baExtra);	// write updated data
			if (status != Successful)
				return status;
		}

#if 0
		for (idx=PageAddr; idx<pMS->uSectorPerBlock; idx++)
		{
			status = msCopyOnePage(pMS, PhyBlockAddr, idx, UnusedPhyBlockAddr, idx);
			if (status != Successful)
				return status;
		}
#endif
	}

	/* 7. update Logical/Physical corresponding information on host product */
	//_ms_gLPTable[LogBlockAddr] = UnusedPhyBlockAddr;

	/* 8. erase block before updated */
	if (PhyBlockAddr != 0xFFFF)
	{
		status = msEraseBlock(pMS, PhyBlockAddr);			
		if (status != Successful)
			return status;
		status = msAllocateFreeTable(PhyBlockAddr);		// add erased block to Free_Table[]
		if (status != Successful)
			return status;
	}
	return Successful;
}

//================================================
//	input:
//		1. LAddr (logical address)
//		2. Count (data Count, 512B pre one unit)
//		3. pUploadBuf (upload pointer)
//================================================
int msReadMultiPages(FMI_MS_INFO_T *pMS, UINT32 LAddr, int Count, char *pUploadBuf)
{
	char baBuf[8];
	int volatile i, status;
	unsigned int reg;

	if (_ms_gMSProWriteRegSize != 0x06)
	{
		_ms_gMSProWriteRegSize = 0x06;

		baBuf[0] = 0x01;	// read address
	    baBuf[1] = 0x08;	// read size
	    baBuf[2] = 0x11;	// write address
	    baBuf[3] = 0x06;	// write size

	    /* send SET_R/W_REG_ADRS Command */
	    msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	}

	outpw(REG_MSBUF2, (inpw(REG_MSBUF2)&0xff0000ff) | (Count<<8));
	outpw(REG_MSBUF2, (inpw(REG_MSBUF2)&0xffffff00) | ((LAddr&0xff000000)>>24));
	outpw(REG_MSBUF1, (inpw(REG_MSBUF1)&0x000000ff) | ((LAddr&0x00ffffff)<<8));

    /* send READ_DATA command */
	msExSetCMD(MS_READ_DATA);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	for(i=0; i<Count; i++)
	{
		/* wait INT ready */
		if((status = msWaitINT(pMS)) != Successful) return status;

		if (pMS->bIsMSProParallel == TRUE)
		{
			reg = inpw(REG_MSISR);			// INT register
			if(reg & 0x800) return FMI_MS_INT_CMDNK;		// Command error
			if(reg & 0x200) return FMI_MS_INT_ERR;		// Read error
			if(!(reg & 0x400)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?
 		}
 		else
 		{
			/* check INT register */
			msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
			reg = *baBuf;		// INT register

			if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
			if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error
			if(!(reg & 0x20)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?
		}

		msSendTPC(pMS, MS_READ_LONG_DATA, 0x00, pUploadBuf);	// receive data from data buffer

		/* wait Packet transferred */
		while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
		{
			if (pMS->bIsCardInsert == FALSE)
			{
				outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
				return FMI_NO_MS_CARD;
			}
		}

		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
		pUploadBuf += 0x200;
	}

    /* send STOP command */
	msExSetCMD(MS_STOP);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful) return status;

	return Successful;
}

//=================================================
//	input:
//		1. LAddr (logical address)
//		2. Count (data Count, 512B pre one unit)
//		3. pLoadBuf (load pointer)
//=================================================
int msWriteMultiPages(FMI_MS_INFO_T *pMS, UINT32 LAddr, int Count,  char *pLoadBuf)
{
	char baBuf[8];
	int volatile i, status;
	unsigned int reg;

	if (_ms_gMSProWriteRegSize != 0x06)
	{
		_ms_gMSProWriteRegSize = 0x06;

		baBuf[0] = 0x01;	// read address
		baBuf[1] = 0x08;	// read size
		baBuf[2] = 0x11;	// write address
		baBuf[3] = 0x06;	// write size

		/* send SET_R/W_REG_ADRS Command */
		msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

		/* check RDY signal */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	}

	outpw(REG_MSBUF2, (inpw(REG_MSBUF2)&0xff0000ff) | (Count<<8));
	outpw(REG_MSBUF2, (inpw(REG_MSBUF2)&0xffffff00) | ((LAddr&0xff000000)>>24));
	outpw(REG_MSBUF1, (inpw(REG_MSBUF1)&0x000000ff) | ((LAddr&0x00ffffff)<<8));

	/* send WRITE_DATA command */
	msExSetCMD(MS_WRITE_DATA);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	for(i=0; i<Count; i++)
	{
		/* wait INT ready */
		if((status = msWaitINT(pMS)) != Successful) return status;

		if (pMS->bIsMSProParallel == TRUE)
		{
			reg = inpw(REG_MSISR);			// INT register
			if(reg & 0x800) return FMI_MS_INT_CMDNK;		// Command error
			if(reg & 0x200) return FMI_MS_INT_ERR;		// Read error
			if(!(reg & 0x400)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?
 		}
 		else
 		{
			/* check INT register */
			msSendTPC(pMS, MS_GET_INT, 0x01, baBuf);
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
			reg = *baBuf;		// INT register

			if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
			if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error
			if(!(reg & 0x20)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?
		}

		msSendTPC(pMS, MS_WRITE_LONG_DATA, 0x00, pLoadBuf);	// send data from data buffer

		/* wait Packet transferred */		              
		while(inpw(REG_DMACCSR) & 0x200)	// wait DMAC FMI ready
		{
			if (pMS->bIsCardInsert == FALSE)
			{
				outpw(REG_DMACCSR, inpw(REG_DMACCSR) | 0x02);
				return FMI_NO_MS_CARD;
			}
		}
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		pLoadBuf += 0x200;
	}

    /* send STOP command */
	msExSetCMD(MS_STOP);
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
	if((status = msWaitINT(pMS)) != Successful) return status;

	return Successful;
}

int fmiMS_Read_in(FMI_MS_INFO_T *pMS, UINT32 SectorNo, UINT32 SectorCount, char *pUploadBuf)
{
#ifdef DMAC_SCATTER_GETTER
	outpw(REG_DMACCSR, 0x00000001);
#endif

	if (pMS->uCardType == FMI_MSPRO_TYPE)
		return msReadMultiPages(pMS, SectorNo, SectorCount, pUploadBuf);
	else	// memory stick
		return msReadLogicalPages(pMS, SectorNo, SectorCount, pUploadBuf);
	return Successful;
}

int fmiMS_Write_in(FMI_MS_INFO_T *pMS, UINT32 SectorNo, UINT32 SectorCount, char *pLoadBuf)
{
#ifdef DMAC_SCATTER_GETTER
	outpw(REG_DMACCSR, 0x00000001);
#endif

	if (pMS->uCardType == FMI_MSPRO_TYPE)
		return msWriteMultiPages(pMS, SectorNo, SectorCount, pLoadBuf);
	else	// memory stick
		return msWriteLogicalPages(pMS, SectorNo, SectorCount, pLoadBuf);
	return Successful;
}

int fmiMS_Initial(FMI_MS_INFO_T *pMS)
{
	char *pPtr;
	char baBuf[8], baExtra[4];
	unsigned int reg;
	int volatile i, j, entryCount, status;
	unsigned int volatile tick;

	//---------------------------------------------	
	// enable MS signal and set related registers
	//---------------------------------------------
	sysFlushCache(I_D_CACHE);
	sysSetCachePages((UINT32)_ms_gBuffer, 512, CACHE_DISABLE);
	_ms_gpCopyPage = (char *)((UINT32)_ms_gBuffer | 0x80000000);

	outpw(REG_MSIER, inpw(REG_MSIER) | 0x01);

	//-------------------------------------------------------	
	// wait for MS card I/F startUp time (10 ms at least)
	//-------------------------------------------------------	
	// orignal: 150 ms
	tick = sysGetTicks(TIMER0);
	while(1)
	{
		if ((sysGetTicks(TIMER0) - tick) > 15)
			break;
	}

	//========================================
	//	Medis type identification process 
	//		- MS pro or MS card is inserted.
	//========================================
	baBuf[0] = 0x00;	// read address
	baBuf[1] = 0x08;	// read size
	baBuf[2] = 0x10;	// write address
	baBuf[3] = 0x01;	// write size

	/* send SET_R/W_REG_ADRS Command */
	msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

	/* check RDY signal */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* send READ_REG Command */
	msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);

	/* check CRC ready */
	if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

	/* identify MS pro or MS type card */
	reg = *(baBuf+4);		// get TYPE register
	if(reg==0x01)			// chek if MS PRO type
	{
		if(*(baBuf+6)==0x00)	// check Category Reg = 0x00
		{
			switch (*(baBuf+7))
			{
				// MSPro, parallel
				case 0:
				case 1:
				case 2:
				case 3:
#ifdef DEBUG
	printf("1. type[%x], category[%x], class[%x], WP[%x]\n",*(baBuf+4), *(baBuf+6), *(baBuf+7), *(baBuf+2));
#endif
					pMS->uCardType = FMI_MSPRO_TYPE;
					pMS->bIsMSProParallel = TRUE;
					//pMS->bIsMSProParallel = FALSE;
					break;
				default:
					return FMI_MS_WRONG_TYPE;
			}
		}
		else 	return FMI_MS_WRONG_TYPE;
	}
	else if (reg==0x00)				// check if MS type
	{
		if(*(baBuf+6)==0x00)			// check Category Reg = 0x00
		{
			switch (*(baBuf+7))
			{
				// memory stick, serial
				case 0:
				case 1:
				case 2:
				case 3:
#ifdef DEBUG
	printf("2. type[%x], category[%x], class[%x]\n",*(baBuf+4), *(baBuf+6), *(baBuf+7));
#endif
					pMS->uCardType = FMI_MS_TYPE;
					pMS->bIsMSProParallel = FALSE;
					break;
				default:
					return FMI_MS_WRONG_TYPE;
			}
		}
		else 	return FMI_MS_WRONG_TYPE;
	}
	else if (reg==0xFF)				// identify MS type
	{
		if(*(baBuf+6)==0xFF)			// check Category Reg = 0x00
		{
			switch (*(baBuf+7))
			{
				// memory stick, serial
				case 0xff:
				case 1:
				case 2:
				case 3:
#ifdef DEBUG
	printf("3. type[%x], category[%x], class[%x]\n",*(baBuf+4), *(baBuf+6), *(baBuf+7));
#endif
					pMS->uCardType = FMI_MS_TYPE;
					pMS->bIsMSProParallel = FALSE;
					break;
				default:
					return FMI_MS_WRONG_TYPE;
			}
		}
		else 	return FMI_MS_WRONG_TYPE;
	}
	else 	return FMI_MS_WRONG_TYPE;

	//===============================================
	//	3a. CPU startUp Process
	//		- get INT reg to confirm MS pro is ready.
	//		- should be finished by 10 sec.
	//===============================================
	if (pMS->uCardType == FMI_MSPRO_TYPE)
	{
		do
		{
			for(i=0; i<100; i++)
			{
				/* get INT register */
				reg = *(baBuf+1);

				if(reg & 0x80)	// CED is OK
				{
					/* send READ_REG Command */
					msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);
					if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

					/* get INT register */
					reg = *(baBuf+1);
					if (!(reg & 0x40))	break;
				}

				// delay 100 ms
				tick = sysGetTicks(TIMER0);
				while (1)
				{
					if ((sysGetTicks(TIMER0) - tick) > 10)
						break;
				}

				/* send READ_REG Command */
				msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);
				if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
			}
			if(i>100)	return FMI_MS_INIT_ERROR;

			if(reg & 0x40)	// error
			{
				if(reg & 0x01) return Successful; // Write-disabled status
				return FMI_MS_WRITE_DISABLE;
			}
			break;

		} while(1);

	//=======================================================
	//	4a. Procedure to switch interface mode 
	//		- switch to parallel mode or serial mode
	//			> set System Parameter Reg in MS pro card
	//			> set CPU related registers
	//=======================================================
		if (pMS->bIsMSProParallel == TRUE)
		{
			baBuf[0] = 0x00;	// b8=0 -> parallel mode
			msSendTPC(pMS, MS_WRITE_REG, 0x01, baBuf);

			/* check RDY signal */
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		    /* selecte parallel mode */
		    /* select MS pro */
			outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfffffff3) | 0x04);

			/* send READ_REG Command */
			msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
		}
		else	// serial
		{
			baBuf[0] = 0x80;	// b8=1 -> serial mode
			msSendTPC(pMS, MS_WRITE_REG, 0x01, baBuf);

			/* check RDY signal */
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		    /* selecte serial mode */
		    /* select MS pro */
			outpw(REG_MSCSR, (inpw(REG_MSCSR)&0xfffffff3) | 0x0c);
		}

	//=======================================================
	//	5a. Procedure to obtain Attribute Information
	//		- send EX_SET_CMD TPC (READ_ATRB CMD)
	//=======================================================
		// DataCountReg = 0x0001, DataAddrReg = 0x000000
		outpw(REG_MSBUF2, 0x00000100);
		outpw(REG_MSBUF1, 0x00000000);

		msExSetCMD(MS_READ_ATRB);

		/* wait INT ready */
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
		if((status = msWaitINT(pMS)) != Successful) return status;

		if (pMS->bIsMSProParallel == TRUE)
		{
			reg = inpw(REG_MSISR);
			if(reg & 0x800) return FMI_MS_INT_CMDNK;		// Command error
			if(reg & 0x200) return FMI_MS_INT_ERR;		// Read error
			if(!(reg & 0x400)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?

			/* receive data from data buffer */
			msSendTPC(pMS, MS_READ_LONG_DATA, 0x00, _ms_gpCopyPage);

			/* check RDY signal */
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;
		}
		else
		{
			/* send READ_REG Command */
			msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

			/* check BREQ siganl */
			reg = *(baBuf+1);				// get INT register
			if(reg & 0x01) return FMI_MS_INT_CMDNK;		// Command error
			if(reg & 0x40) return FMI_MS_INT_ERR;		// Read error
			if(!(reg & 0x20)) return FMI_MS_INT_BREQ;	// Data is ready in Data Buffer?

			/* receive data from data buffer */		                                    																																																							
			msSendTPC(pMS, MS_READ_LONG_DATA, 0x00, _ms_gpCopyPage);                               																																																							

			/* check RDY signal */
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

			/* send READ_REG Command */
			msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

			/* check CED siganl */
			reg = *(baBuf+1);				// get INT register
			if(!(reg & 0x80)) return FMI_MS_INT_CED_ERR;		// Commad finished?
		}

	//====================================================================
	//	6a. Procedure to confirm Attribute Information
	//		- signature code is 0xA5C3 ?
	//		- check Device Information Entry count A is: 0<A<13?
	//			> read out Sytem Information 
	//			> Address of Entry N is equal or greater than 0x1A0? 
	//			> Address + Size of Entry N is equal or less than 0x8000?
	//		- confirm System Information
	//====================================================================
		pPtr = (char *)&_ms_gMSProDeviceEntry;

#ifdef DEBUG
	printf("attribute info [%x][%x]\n", *_ms_gpCopyPage, *(_ms_gpCopyPage+1));
#endif
		if ((*_ms_gpCopyPage != 0xA5) && (*(_ms_gpCopyPage+1) != 0xC3))
			return FMI_MS_INIT_ERROR;		// check the signature code

		/* find out System Information */
		// please refer to Memory Stick Pro ver. 1.02-00, page-128
		entryCount = *(_ms_gpCopyPage+4);
		if (entryCount > 12)
			return FMI_MS_INIT_ERROR;	// error entry count

		for(i=0; i<entryCount; i++)
		{
			for(j=0; j<12; j++)
			{
				*(pPtr+j) = *(_ms_gpCopyPage+(16 + i*12 + j));
			}
			if(_ms_gMSProDeviceEntry.msDeviceID == 0x10)
			{
				_ms_gMSProDeviceEntry.msAddr = Swap32(_ms_gMSProDeviceEntry.msAddr);
				_ms_gMSProDeviceEntry.msSize = Swap32(_ms_gMSProDeviceEntry.msSize);
				break;
			}
		}

		/* read System Information starting address */
		if(_ms_gMSProDeviceEntry.msAddr > 0x8000)	return FMI_MS_INIT_ERROR;

		/* read System Information data size (in Byt) */		
		if(_ms_gMSProDeviceEntry.msSize != 96)	return FMI_MS_INIT_ERROR;	// size shall be 96 bytes
	
		if((_ms_gMSProDeviceEntry.msAddr == 0x01A0))			// just after Attribute Information Area (size = 0x01A0 bytes)
		{
			/* System Information in current read page data */
			// 416 + 96 = 512 
			for(i=0; i<96; i++)
			{
				pPtr = (char *)&_ms_gMSProSystemInfo; 
				*(pPtr+i) = *(_ms_gpCopyPage+(416+i));
			}

			// Mmemory Stick class
			// _ms_gBuffer[416]
#ifdef DEBUG
			printf("Memory Stick class [%d]\n", _ms_gMSProSystemInfo.msClass);
#endif
			// Block size
			// _ms_gBuffer[419 - 418]
			_ms_gMSProSystemInfo.msBlockSize = Swap16(_ms_gMSProSystemInfo.msBlockSize);

			// Total blocks
			// _ms_gBuffer[421 - 420]
			_ms_gMSProSystemInfo.msTotalBlock = Swap16(_ms_gMSProSystemInfo.msTotalBlock);
			_ms_gMSProSystemInfo.msUserAreaBlock = Swap16(_ms_gMSProSystemInfo.msUserAreaBlock);
			_ms_gMSProSystemInfo.msPageSize = Swap16(_ms_gMSProSystemInfo.msPageSize);
			_ms_gMSProSystemInfo.msStartSec = Swap16(_ms_gMSProSystemInfo.msStartSec);
			_ms_gMSProSystemInfo.msUnitSize = Swap16(_ms_gMSProSystemInfo.msUnitSize);
		}
		else
		{
			/* get System Information in other address */
			// will be add in future
		}

	//=======================================================
	//	7a. Procedure to recognize File System.
	//		- read out LBA=0 sector
	//		- search MBR and PBR
	//		- confirm File System from PBR.
	//=======================================================

	} // end memory stick pro

	//=======================================================
	//	3b. Boot block search
	//		- search Boot block
	//		- Boot block No. is more than 12? 
	//			> Boot block is equal or less than 12
	//		- Shall 2 Boot block can be found.
	//=======================================================
	else // memory stick
	{
		/* send RESET command */
		msSetCMD(MS_RESET);	// no INT in RESET cmd
		if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

		_ms_gBootBlockIndex = -1;
		for(i=0; i<12; i++)
		{
			memset(_ms_gpCopyPage, 0, 512);
			status = msReadOnePage(pMS, i, 0x00, _ms_gpCopyPage, baExtra);

			if(!status)				// check if Good block
			{
				baBuf[0] = 0x15;	// read address
				baBuf[1] = 0x08;	// read size
				baBuf[2] = 0x10;	// write address
				baBuf[3] = 0x01;	// write size

				/* send SET_R/W_REG_ADRS Command */
				msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

				if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

				msSendTPC(pMS, MS_READ_REG, 0x08, baBuf);	// read register address = 0x15
														// register size = 0x08
				/* check RDY signal */
				if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

				reg = *(baBuf+2);				// get Management Flag register

  				/* check if current is Boot Block */
				if(!(reg & 0x04))		// management flag register
				{
					if ((*_ms_gpCopyPage==0x00) && (*(_ms_gpCopyPage+1)==0x01))
					{
						_ms_gBootBlockIndex++;	// g_bBootBlockIndex will record 2nd Boot Block
						if (_ms_gBootBlockIndex == 1)
							break;					// two Boot Blocks can be found
					}
			    }
			}
		}

		/* check if Boot Block is more than 12 */
		if(i > 11) return FMI_MS_INIT_ERROR;	// bad MS card

	//==================================================================
	//	4b. Check Boot block contents 
	//		- If Parallel mode is supported, switch to Parallel mode.
	//		- check total blocks and block size
	//==================================================================

		/* check if parallel mode supported */
		reg = *(_ms_gpCopyPage+0x1D3);
#if 0
		if(reg==1)						// Serial=0, Serial/Parallel=1
		{
			pMS->bIsMSProParallel = TRUE;
			baBuf[0] = 0x00;		// read address
			baBuf[1] = 0x08;		// read size
			baBuf[2] = 0x10;		// write address
			baBuf[3] = 0x01;		// write size

			/* send SET_R/W_REG_ADRS Command */
			msSendTPC(pMS, MS_SET_RW_REG_ADRS, 0x04, baBuf);

			/* check RDY signal */
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

			/* set System Parameter Reg in MS card */
			/* set CPU related registers */

			baBuf[0] = 0x88;			// b8=1, b4=1 ==> parallel mode
			msSendTPC(pMS, MS_WRITE_REG, 0x01, baBuf);	// write size = 1

			/* check RDY signal */
			if((status = msWaitPacketTransfer(pMS)) != Successful) return status;

			/* selecte transfer mode */
			// parallel mode selected
			outpw(REG_MSCSR, inpw(REG_MSCSR)&0xfffffff3);
		}
#endif
		/* check total blocks and block size */
		pMS->uBlockSize = *(_ms_gpCopyPage+0x1A3);	// block size: 0x10(16KB) / 0x08(8KB)
		pMS->uSectorPerBlock = pMS->uBlockSize * 2; // sector count per block : 0x20 / 0x10 sectors

		pMS->uBlockPerFlash = (*(_ms_gpCopyPage+0x1A6)<<8)|*(_ms_gpCopyPage+0x1A7);

	//===========================================================================
	//	5b. Create Logical to Physical Block Number Corresponding Information
	//===========================================================================
		_ms_gSegCount = pMS->uBlockPerFlash / 512;
		_ms_gSegIndex = 0;
		msMakeTable(pMS, _ms_gSegIndex);		// create Segment-0 LP mapping table

		/* read Disabled Block Data */
		status = msReadOnePage(pMS, _ms_gBootBlockIndex, 0x01, _ms_gpCopyPage, baExtra);
		if(status != Successful) return status;

	//==========================================================
	//	6b. - Store "CIS(256B)+IDI(256B) in bpDMAStartAddr[512].
	//==========================================================

		/* read CIS(256B)+IDI(256B) */
		status = msReadOnePage(pMS, _ms_gBootBlockIndex, 0x02, _ms_gpCopyPage, baExtra);
		if(status != Successful) return status;

	} // end memory stick

	// return total sectors
	if (pMS->uCardType == FMI_MSPRO_TYPE)
	{
		pMS->uBlockPerFlash = _ms_gMSProSystemInfo.msUserAreaBlock;
		pMS->uSectorPerFlash = _ms_gMSProSystemInfo.msBlockSize * _ms_gMSProSystemInfo.msUserAreaBlock;
	}
	else
	{
		pMS->uSectorPerFlash = pMS->uSectorPerBlock * pMS->uBlockPerFlash;
	}

	return Successful;
}

VOID fmiGet_MS_info(FMI_MS_INFO_T *pMS, DISK_DATA_T *_info)
{
	_info->diskSize =  pMS->uSectorPerFlash / 2;
	_info->sectorSize = 512;
	_info->totalSectorN = pMS->uSectorPerFlash;

	_info->vendor[0] = '\0';
	_info->product[0] = '\0';
	_info->serial[0] = '\0';
}


